#include "stm32f10x.h"                  // Device header
#include "Serial.h"
#include "ServoControl.h"

int jiaozhun[19]={0,0+90,0+90-0,90+0,0+90,0+90-0,0+90,0+90,-5+90,0+90,0+90,0+90+0,90+5,0+90,0+90+0,90+5,0+90,0+90-5,90-5};


void SC_SetAngle( uint8_t Servo, int16_t angle)
{
	if( Servo < 0x10)
	{
		Serial_Tx_Data[0] = Servo;
		Serial_Tx_Data[1] = angle;
		Serial_SendPacket_1();
	}
	if( Servo > 0x90)
	{
		Serial_Tx_Data[0] = Servo;
		Serial_Tx_Data[1] = angle;
		Serial_SendPacket_2();
	}
}

void SC_SetLegL1(int16_t angle1,int16_t angle2,int16_t angle3)
{
	SC_SetAngle( Servo_L1, angle1+jiaozhun[1]);
	SC_SetAngle( Servo_L2, angle2+jiaozhun[2]);
	SC_SetAngle( Servo_L3, angle3+jiaozhun[3]);
}

void SC_SetLegL2(int16_t angle1,int16_t angle2,int16_t angle3)
{
	SC_SetAngle( Servo_L4, angle1+jiaozhun[4]);
	SC_SetAngle( Servo_L5, angle2+jiaozhun[5]);
	SC_SetAngle( Servo_L6, angle3+jiaozhun[6]);
}

void SC_SetLegL3(int16_t angle1,int16_t angle2,int16_t angle3)
{
	SC_SetAngle( Servo_L7, angle1+jiaozhun [7]);
	SC_SetAngle( Servo_L8, angle2+jiaozhun [8]);
	SC_SetAngle( Servo_L9, angle3+jiaozhun [9]);
}

void SC_SetLegR1(int16_t angle1,int16_t angle2,int16_t angle3)
{
	SC_SetAngle( Servo_R1, -angle1+jiaozhun [10]);
	SC_SetAngle( Servo_R2, -angle2+jiaozhun [11]);
	SC_SetAngle( Servo_R3, -angle3+jiaozhun [12]);
}

void SC_SetLegR2(int16_t angle1,int16_t angle2,int16_t angle3)
{
	SC_SetAngle( Servo_R4, -angle1+jiaozhun [13]);
	SC_SetAngle( Servo_R5, -angle2+jiaozhun [14]);
	SC_SetAngle( Servo_R6, -angle3+jiaozhun [15]);
}

void SC_SetLegR3(int16_t angle1,int16_t angle2,int16_t angle3)
{
	SC_SetAngle( Servo_R7, -angle1+jiaozhun [16]);
	SC_SetAngle( Servo_R8, -angle2+jiaozhun [17]);
	SC_SetAngle( Servo_R9, -angle3+jiaozhun [18]);
}
